#include "stdafx.h"
#include "FingerTipsDetector.h"
#include "comment.h" // opencv header
#include "PointsClassifier.h"
#include <list>
#include "VecCalculator.h"
#include "HandOrientation.h"

namespace ftips {

	// Function: to detect fingertips
	bool FingerTipsDetector::DetectFingerTips(CvSeq *pContour, std::list<CvPoint> *pTipsList, std::list<CvPoint> *pGroovesList) {
		int total = pContour->total; 
		if (total < MINK) // If the contour too small, give it up
			return false;

		PointsClassifier pTipsClassifier = PointsClassifier(total); 
		PointsClassifier pGroovesClassifier = PointsClassifier(total);

		// To traverse all points in contour
		for( int i = 0; i <= total; i++ ) {	
			CvPoint *p = (CvPoint *)cvGetSeqElem(pContour, i); 
			double minCos = 0; 
			CvPoint *pBeforeK;
			CvPoint *pAfterK;

			for (int k = MINK; k < MAXK; k++) {
				int beforeKPos = ((i - k) < 0) ? i + total - k : i - k; 
				int afterKPos = ((i + k) > total) ? i + k - total : i + k;

				pBeforeK = (CvPoint *)cvGetSeqElem(pContour, beforeKPos); 
				pAfterK = (CvPoint *)cvGetSeqElem(pContour, afterKPos); 

				double cos = VecCalculator::CalCosineValue(p, pBeforeK, pAfterK);
				minCos = cos < minCos ? cos : minCos ; // To get the minus cos value in the range
			}

			if (minCos < (COS_THEESHOLD / 10.0)) {
				if (!VecCalculator::CalCrossProduct(p, pBeforeK, pAfterK)) 
					pGroovesClassifier.Kmedoids(i); // groove point
				else pTipsClassifier.Kmedoids(i);	// tip points
			}
		}	

		// Copy tips point to the finger tips list
		GetTipPointsList(pContour, &pTipsClassifier, pTipsList);

		// Copy grooves point to the grooves list
		GetGroovePointsList(pContour, &pGroovesClassifier, pGroovesList);
		
		// Calculate hand orientation
		CvPoint orientation = HandOrientation::GetOrientationAndFiltTips(pContour, &pTipsClassifier, pTipsList, MAXK);
	
		return true;
	}

	// Function: get center point of finger tip
	bool FingerTipsDetector::CalTipPointsCenter(std::list<CvPoint> *pPointsList, CvPoint *pCenter) {
		std::list<CvPoint>::iterator iter;
		int sumX = 0;
		int sumY = 0;
		for (iter = pPointsList->begin(); iter != pPointsList->end(); ++iter) {
			sumX += iter->x;
			sumY += iter->y;
		}
		pCenter->x = sumX / pPointsList->size();
		pCenter->y = sumY / pPointsList->size();
		return true;
	}

	// Function: copy groove points to list
	void FingerTipsDetector::GetGroovePointsList(CvSeq *pContour, PointsClassifier *pClassifier, std::list<CvPoint> *pointsPosList) {
		pointsPosList->clear(); // make sure the list is clean
		for (int i = 0; i < pClassifier->GetClassNum(); i++) {
			CvPoint *p = (CvPoint *)cvGetSeqElem(pContour, pClassifier->GetClassCenter(i));
			pointsPosList->push_back(*p); // insert into list
		}
	}

	// Function: get tip points list
	void FingerTipsDetector::GetTipPointsList(CvSeq *pContour, PointsClassifier *pClassifier,
		std::list<CvPoint> *pTipsList) {
		pTipsList->clear(); // make sure the list is clean

		std::list<int> pointsIndexList;
		std::list<CvPoint> pointsPosList;
		CvPoint centerPoint;		// center point on the line
		// To traverse all classes
		for (int i = 0; i < pClassifier->GetClassNum(); ++i) {	
			pointsIndexList.clear();
			pointsPosList.clear();
			pClassifier->GetClassIndividualPoints(i, &pointsIndexList);
			GetPointsListFromIndex(pContour, pClassifier, &pointsIndexList, &pointsPosList);
			CalTipPointsCenter(&pointsPosList, &centerPoint);
			pTipsList->push_back(centerPoint);
		}
	}

	// Function: get points position list from points index
	void FingerTipsDetector::GetPointsListFromIndex(CvSeq *pContour, PointsClassifier *pClassifier,
		std::list<int> *pointsIndexList, std::list<CvPoint> *pointsPosList) {

		std::list<int>::iterator iterIndex;
		for (iterIndex = pointsIndexList->begin(); iterIndex != pointsIndexList->end(); ++iterIndex) {
			CvPoint *p = (CvPoint *)cvGetSeqElem(pContour, *iterIndex);
			pointsPosList->push_back(*p);
		}
	}

	// Function: release memory
	FingerTipsDetector::~FingerTipsDetector() {
	}

}